#include "UserConfiguration.h" // Edit this file first before uploading to the AeroQuad
#include <EEPROM.h>
#include <Wire.h>
#include <GlobalDefined.h>
#include "AeroQuad.h"
#include "PID.h"
#include <AQMath.h>
#include <FourtOrderFilter.h>
#include <Receiver.h>

//********* PLATFORM SPECIFIC SECTION ********************
#define __AVR_ATmegaUNO__

#define LED_Green 13
#define LED_Red 12
#define LED_Yellow 12
#include <Device_I2C.h>
#define ITG3200_ADDRESS_ALTERNATE
#include <Gyroscope_ITG3200_9DOF.h>
#include <Accelerometer_ADXL345_9DOF.h>
#define RECEIVER_328P
#define MOTOR_PWM_Timer
#define SPARKFUN_9DOF_5883L
#undef BattMonitorAutoDescent
#undef BattCellCount
#undef POWERED_BY_VIN
#undef AltitudeHoldBaro
#undef AltitudeHoldRangeFinder
#undef CameraControl
#undef OSD

/**
 * Put AeroQuad_v18 specific intialization need here
 */
void initPlatform() {
    pinMode(LED_Red, OUTPUT);
    digitalWrite(LED_Red, LOW);
    pinMode(LED_Yellow, OUTPUT);
    digitalWrite(LED_Yellow, LOW);
    Wire.begin();
    TWBR = 12;
}

/**
 * Measure critical sensors
 */
void measureCriticalSensors() {
    measureAccelSum();
    measureGyroSum();
}

#include "Kinematics.h"
#include "Kinematics_DCM.h"
#include <Receiver_328p.h>
#include <Motors_PWM_Timer.h>
#include <Magnetometer_HMC5883L.h>
#include "FlightControlQuadX.h"
#define SERIAL_PORT Serial
#include "AltitudeControlProcessor.h"
#include "FlightControlProcessor.h"
#include "FlightCommandProcessor.h"
#include "HeadingHoldProcessor.h"
#include "DataStorage.h"
#include "SerialCom.h"

/**
 * Main setup function, called one time at bootup
 * initalize all system and sub system of the
 * Aeroquad
 */
void setup() {
    SERIAL_BEGIN(BAUD);
    pinMode(LED_Green, OUTPUT);
    digitalWrite(LED_Green, LOW);

#ifdef CHANGE_YAW_DIRECTION
    YAW_DIRECTION = -1;
#endif

    // Read user values from EEPROM
    readEEPROM(); // defined in DataStorage.h
    if (readFloat(SOFTWARE_VERSION_ADR) != SOFTWARE_VERSION) {
        // If we detect the wrong soft version, we init all parameters
        initializeEEPROM();
        writeEEPROM();
    }

    initPlatform();
    initializeMotors(FOUR_Motors);

    // Initialize max/min values for all motors
    for (byte motor = 0; motor < LASTMOTOR; motor++) {
        motorMinCommand[motor] = minArmedThrottle;
        motorMaxCommand[motor] = MAXCOMMAND;
    }

    // Setup receiver pins for pin change interrupts
    initializeReceiver(LASTCHANNEL);
    initReceiverFromEEPROM();

    // Initialize sensors
    // If sensors have a common initialization routine
    // insert it into the gyro class because it executes first
    initializeGyro(); // defined in Gyro.h
    initializeAccel(); // defined in Accel.h
    initSensorsZeroFromEEPROM();

    // Calibrate sensors
    calibrateGyro();
    computeAccelBias();
    zeroIntegralError();

    // Flight angle estimation
    vehicleState |= HEADINGHOLD_ENABLED;
    initializeMagnetometer();
    initializeKinematics(getHdgXY(XAXIS), getHdgXY(YAXIS));

    // Integral Limit for attitude mode
    // This overrides default set in readEEPROM()
    // Set for 1/2 max attitude command (+/-0.75 radians)
    // Rate integral not used for now
    PID[ATTITUDE_XAXIS_PID_IDX].windupGuard = 0.375;
    PID[ATTITUDE_YAXIS_PID_IDX].windupGuard = 0.375;
    setupFourthOrder();
    previousTime = micros();
    digitalWrite(LED_Green, HIGH);
    safetyCheck = 0;
}

/*******************************************************************
  // tasks (microseconds of interval)
  ReadGyro        readGyro      (   5000); // 200hz
  ReadAccel       readAccel     (   5000); // 200hz
  RunDCM          runDCM        (  10000); // 100hz
  FlightControls  flightControls(  10000); // 100hz
  ReadReceiver    readReceiver  (  20000); //  50hz
  ReadBaro        readBaro      (  40000); //  25hz
  ReadCompass     readCompass   ( 100000); //  10Hz
  ProcessTelem    processTelem  ( 100000); //  10Hz
  ReadBattery     readBattery   ( 100000); //  10Hz
  Task *tasks[] = {&readGyro, &readAccel, &runDCM, &flightControls,   \
                   &readReceiver, &readBaro, &readCompass,            \
                   &processTelem, &readBattery};
  TaskScheduler sched(tasks, NUM_TASKS(tasks));
  sched.run();
 *******************************************************************/
void loop() {
    currentTime = micros();
    deltaTime = currentTime - previousTime;
    measureCriticalSensors();
    // Main scheduler loop set for 100hz
    if (deltaTime >= 10000) {
        frameCounter++;
        // ================================================================
        // 100hz task loop
        // ================================================================
        if (frameCounter % TASK_100HZ == 0) { //  100 Hz tasks
            G_Dt = (currentTime - hundredHZpreviousTime) / 1000000.0;
            hundredHZpreviousTime = currentTime;
            evaluateMetersPerSec();
            evaluateGyroRate();
            for (int axis = XAXIS; axis <= ZAXIS; axis++) {
                filteredAccel[axis] = computeFourthOrder(meterPerSecSec[axis], &fourthOrder[axis]);
            }
            // ****************** Calculate Absolute Angle *****************
            calculateKinematics(gyroRate[XAXIS],
                    gyroRate[YAXIS],
                    gyroRate[ZAXIS],
                    filteredAccel[XAXIS],
                    filteredAccel[YAXIS],
                    filteredAccel[ZAXIS],
                    accelOneG,
                    getHdgXY(XAXIS),
                    getHdgXY(YAXIS),
                    G_Dt);
            // Evaluate are here because we want it to be synchronized with the processFlightControl
            // Combines external pilot commands and measured sensor data to generate motor commands
            processFlightControl();
        }
        // ================================================================
        // 50hz task loop
        // ================================================================
        if (frameCounter % TASK_50HZ == 0) { //  50 Hz tasks
            G_Dt = (currentTime - fiftyHZpreviousTime) / 1000000.0;
            fiftyHZpreviousTime = currentTime;
            // Reads external pilot commands and performs functions based on stick configuration
            readPilotCommands(); // defined in FlightCommand.pde
        }
        // ================================================================
        // 10hz task loop
        // ================================================================
        if (frameCounter % TASK_10HZ == 0) { //   10 Hz tasks
            G_Dt = (currentTime - tenHZpreviousTime) / 1000000.0;
            tenHZpreviousTime = currentTime;

            measureMagnetometer(kinematicsAngle[XAXIS], kinematicsAngle[YAXIS]);

            // Listen for configuration commands and reports telemetry
            readSerialCommand(); // defined in SerialCom.pde
            sendSerialTelemetry(); // defined in SerialCom.pde

        }
        previousTime = currentTime;
    }
    if (frameCounter >= 100) {
        frameCounter = 0;
    }
}
